/* USER CODE BEGIN Header */
/**
 ******************************************************************************
 * File Name          :
 * Description        :
 ******************************************************************************
 * @attention
 *
 *
 ******************************************************************************
 */
/* USER CODE END Header */

/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */
#include "main.h"
/* USER CODE END Includes */

/* Private typedef -----------------------------------------------------------*/
/* USER CODE BEGIN PTD */

/* USER CODE END PTD */

/* Private define ------------------------------------------------------------*/
/* USER CODE BEGIN PD */
#define BRAKE_OPEN LL_GPIO_SetOutputPin(BRAKE_Port, BRAKE_Pin)
#define BRAKE_CLOSE LL_GPIO_ResetOutputPin(BRAKE_Port, BRAKE_Pin)

#define BRAKE_OPEN_CURRENT (0.7f)
#define BRAKE_KEEP_CURRENT (0.2f)
#define Threshold (0.3f)
#define DelayS 0.08f // 80ms
#define DelayMoni 0.02f
/* USER CODE END PD */

/* Private macro -------------------------------------------------------------*/
/* USER CODE BEGIN PM */

/* USER CODE END PM */

/* Private variables ---------------------------------------------------------*/
/* USER CODE BEGIN Variables */
_BrakeState BrakeState;
uint16_t delay;
char MessageReturn = 0;
char ControlFlag = 0;
char delay_detection = 0;
/* USER CODE END Variables */

/* Private function prototypes -----------------------------------------------*/
/* USER CODE BEGIN FunctionPrototypes */

/* USER CODE END FunctionPrototypes */

/* Private application code --------------------------------------------------*/
/* USER CODE BEGIN Application */
void Brake_Control(char on_off)
{
    MessageReturn = 1;

    if (on_off == 0x01)
    {
        ControlFlag = 1;
    }
    else
    {
        ControlFlag = 2;
    }
}
/*
    get Brake state
 */
char BrakeState_Get(void)
{
    return BrakeState;
}
/*
    The Brake is complete , return brake state by CAN
 */
void BrakeStateMessageReturn(bool success)
{
    if (MessageReturn == 1)
    {
        // Minta Protocol
        if (g_sys.RW.UserPara.Para.ProtocolType == ProtocolMinta)
        {
            if (success == true)
                Minta_CAN_MessageSend(S_BRAKE_STATUS, 1, 0, 1);
            else
                Minta_CAN_MessageSend(S_BRAKE_STATUS, BrakeState, 0, 1);
        }

        MessageReturn = 0;
    }
}

void Brake_Routine(uint16_t freq)
{
    // no brake
    if (g_sys.RW.UserPara.Para.BrakeLevel == 0)
    {
        BrakeStateMessageReturn(false);
        return;
    }
    // cycle control
    if (ControlFlag == 2)
    {
        BRAKE_CLOSE;
        ControlFlag = 0;
        delay_detection = freq * DelayMoni;
        goto exit;
    }
    else if (ControlFlag == 1)
    {
        BRAKE_OPEN;
        ControlFlag = 0;
        delay_detection = freq * DelayMoni;
        goto exit;
    }

    if (g_sys.Para.CurrentBrake > (BRAKE_OPEN_CURRENT + Threshold))
    {
        // over current
        BrakeState = B_OVERCURRENT;
    }
    bool brake_enable_state = LL_GPIO_IsOutputPinSet(BRAKE_Port, BRAKE_Pin);
    if (brake_enable_state == true)
    {
        if (g_sys.Para.CurrentBrake > (BRAKE_OPEN_CURRENT + Threshold))
        {
            // over current
            BrakeState = B_LOCKOVERCURRENT;
        }
        else if (g_sys.Para.CurrentBrake < (BRAKE_KEEP_CURRENT - Threshold))
        {
            if (delay_detection > 0)
            {
                delay_detection--;
            }
            else
            {
                // under current
                BrakeState = B_UNDERCURRNET;
            }
        }
    }
    if (BrakeState >= 0xfa) // produce error
    {
        BRAKE_CLOSE;
        BrakeStateMessageReturn(false);
        goto exit;
    }
    // switch brake logic
    float dError;
    if (brake_enable_state == true)
    {
        dError = g_sys.Para.CurrentBrake - BRAKE_OPEN_CURRENT;
        if (fabs(dError) <= Threshold)
        {
            BrakeState = B_UNLOCKing;
        }
        dError = g_sys.Para.CurrentBrake - BRAKE_KEEP_CURRENT;
        if (fabs(dError) <= Threshold)
        {
            BrakeState = B_UNLOCK;
            BrakeStateMessageReturn(true);
        }
    }
    else if (brake_enable_state == false)
    {
        if ((BrakeState == B_UNLOCKing) || (BrakeState == B_UNLOCK))
        {
            BrakeState = B_LOCKing;
        }
        else if (BrakeState == B_LOCKing)
        {
            delay++;
            float delay_count = (float)freq * DelayS;
            if (delay > delay_count)
            {
                delay = 0;
                BrakeState = B_LOCK;
                BrakeStateMessageReturn(true);
            }
        }
        else if (BrakeState == B_LOCK)
        {
            BrakeStateMessageReturn(true);
        }
    }
exit:
    BrakeVolt_Acquire();
}

/* USER CODE END Application */
